#ifndef TOTK1_HPP
#define TOTK1_HPP

#include "fiocv.hpp"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

#include <thread>
#include <mutex>
#include <atomic>
#include <algorithm>

class ToTK1
{
public:
    static void error(const char *msg)
    {
        perror(msg);
        exit(1);
    }
public:
    std::mutex posLock;

    std::vector<cv::Vec2d> pos;

    std::thread th;
    std::atomic<bool> flag;

    std::atomic<bool> idle;

public:
    ToTK1()
    {

    }


    void start(std::string HPath, uint16_t portno=7654)
    {
        int sockfd, newsockfd;
        socklen_t clilen;
        char buffer[256];
        struct sockaddr_in serv_addr, cli_addr;
        int n;

        sockfd = socket(AF_INET, SOCK_STREAM, 0);
        if (sockfd < 0)
            error("ERROR opening socket");
        bzero((char *) &serv_addr, sizeof(serv_addr));

        serv_addr.sin_family = AF_INET;
        serv_addr.sin_addr.s_addr = INADDR_ANY;
        serv_addr.sin_port = htons(portno);

        const int       optVal = 1;
        const socklen_t optLen = sizeof(optVal);

        if(setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void*) &optVal, optLen)!=0)
        {
            error("ERROR on setsockopt");
        }


        if (bind(sockfd, (struct sockaddr *) &serv_addr,
                 sizeof(serv_addr)) < 0)
            error("ERROR on binding");
        listen(sockfd,5);
        clilen = sizeof(cli_addr);




        newsockfd = accept(sockfd,
                           (struct sockaddr *) &cli_addr,
                           &clilen);
        if (newsockfd < 0)
            error("ERROR on accept");


        cv::Mat H;
        if(!Load(H, HPath))
        {
            error("ERROR read H");
        }

        cv::Matx33d HH((double*)H.ptr());

        std::cout<<"HH="<<HH<<"\n";


        while(flag)
        {

            posLock.lock();
            if(pos.empty())
            {
                posLock.unlock();
                if(!idle)
                {
                    n=sprintf(buffer, "[%g,%g,%g,%g,%g,%g]\n", 64.0, 364.0, 121.0, 0.0, 0.0, 0.0);

                    n = write(newsockfd,buffer,n);

                    if (n < 0) {
                        error("ERROR writing to socket");
                        break;
                    }


                    bzero(buffer,256);
                    n = read(newsockfd,buffer,255);
                    if (n < 0) {
                        error("ERROR reading from socket");
                        break;
                    }

                    printf("Here is the message: %s\n",buffer);

                    sleep(8);
                    idle=true;
                }
                else
                {
                    std::this_thread::yield();
                }

            }
            else
            {
                idle=false;
                cv::Vec3d posh=HH*cv::Vec3d(pos[0][0], pos[0][1], 1);
                pos.erase(pos.begin());
                posLock.unlock();

                n=sprintf(buffer, "[%g,%g,%g,%g,%g,%g]\n", posh[0]/posh[2], posh[1]/posh[2], 50.0, 0.0, 0.0, 0.0);

                n = write(newsockfd,buffer,n);

                if (n < 0) {
                    error("ERROR writing to socket");
                    break;
                }


                bzero(buffer,256);
                n = read(newsockfd,buffer,255);
                if (n < 0) {
                    error("ERROR reading from socket");
                    break;
                }

                printf("Here is the message: %s\n",buffer);

                sleep(5);

            }

        }

#if 0
        n=sprintf(buffer, "[%g,%g,%g,%g,%g,%g]\n", 350.0, 153.8, 121.0, 0.0, 0.0, 1.0);

        n = write(newsockfd,buffer,n);

        if (n < 0) {
            error("ERROR writing to socket");
        }


        bzero(buffer,256);
        n = read(newsockfd,buffer,255);
        if (n < 0) {
            error("ERROR reading from socket");
        }

        printf("Here is the message: %s\n",buffer);
#endif



        close(newsockfd);

        close(sockfd);
    }

    static void start_thread(ToTK1 *pdata, std::string HPath, uint16_t portno=7654)
    {
        pdata->start(HPath,portno);
    }

    void Start(std::string HPath, uint16_t portno=7654)
    {
        flag=true;
        idle=true;
        posLock.lock();
        pos.clear();
        posLock.unlock();
        th=std::thread(start_thread, this, HPath, portno);
    }

    void End()
    {
        flag=false;
        if(th.joinable()) th.join();
        idle=true;
    }

    void SetPixel(double px, double py)
    {
        printf("SetPixel: %g, %g\n",px,py);

        posLock.lock();
        pos.push_back(cv::Vec2d(px,py));
        posLock.unlock();
    }

    void WaitUntilMoved()
    {
        posLock.lock();
        bool res=pos.empty();
        posLock.unlock();
        if(!res)
        {
            while(idle)
            {
                std::this_thread::yield();
            }
        }
    }

};

#endif // TOTK1_HPP
